{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "\n",
    "from pydrake.common import temp_directory\n",
    "from pydrake.geometry import StartMeshcat\n",
    "from pydrake.systems.analysis import Simulator\n",
    "from pydrake.visualization import ModelVisualizer\n",
    "from pydrake.all import (\n",
    "    Simulator,\n",
    "    StartMeshcat,\n",
    ")\n",
    "\n",
    "from manipulation import running_as_notebook\n",
    "from manipulation.station import MakeHardwareStation, load_scenario"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer. The cell will output an HTTP link after the execution.\n",
    "# Click the link and a MeshCat tab should appear in your browser.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create a Drake temporary directory to store files.\n",
    "# Note: this tutorial will create a temporary file (table_top.sdf)\n",
    "# in the `/tmp/robotlocomotion_drake_xxxxxx` directory.\n",
    "temp_dir = temp_directory()\n",
    "\n",
    "# Create a table top SDFormat model.\n",
    "table_top_sdf_file = os.path.join(temp_dir, \"table_top.sdf\")\n",
    "table_top_sdf = \"\"\"<?xml version=\"1.0\"?>\n",
    "<sdf version=\"1.7\">\n",
    "\n",
    "  <model name=\"table_top\">\n",
    "    <link name=\"table_top_link\">\n",
    "      <inertial>\n",
    "        <mass>18.70</mass>\n",
    "        <inertia>\n",
    "          <ixx>0.79</ixx>\n",
    "          <ixy>0</ixy>\n",
    "          <ixz>0</ixz>\n",
    "          <iyy>0.53</iyy>\n",
    "          <iyz>0</iyz>\n",
    "          <izz>1.2</izz>\n",
    "        </inertia>\n",
    "      </inertial>\n",
    "    <visual name=\"bottom\">\n",
    "        <pose>0.0 0.0 0.445 0 0 0</pose>\n",
    "        <geometry>\n",
    "          <box>\n",
    "            <size>0.49 0.63 0.015</size>\n",
    "          </box>\n",
    "        </geometry>\n",
    "        <material>\n",
    "          <diffuse>0.9 0.9 0.9 1.0</diffuse>\n",
    "        </material>\n",
    "      </visual>\n",
    "      <collision name=\"bottom\">\n",
    "        <pose>0.0 0.0 0.445 0 0 0</pose>\n",
    "        <geometry>\n",
    "          <box>\n",
    "            <size>0.49 0.63 0.015</size>\n",
    "          </box>\n",
    "        </geometry>\n",
    "        <drake:proximity_properties>\n",
    "          <drake:compliant_hydroelastic/>\n",
    "          <drake:hydroelastic_modulus>1.0e6</drake:hydroelastic_modulus>\n",
    "        </drake:proximity_properties>\n",
    "      </collision>\n",
    "    </link>\n",
    "    <frame name=\"table_top_center\">\n",
    "      <pose relative_to=\"table_top_link\">0 0 0.47 0 0 0</pose>\n",
    "    </frame>\n",
    "  </model>\n",
    "</sdf>\n",
    "\n",
    "\"\"\"\n",
    "\n",
    "with open(table_top_sdf_file, \"w\") as f:\n",
    "    f.write(table_top_sdf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Adding Models \n",
    "\n",
    "In this exercise, you will learn the basics of adding objects into Drake. You will learn about URDFs, SDFs, and obj files. \n",
    "\n",
    "Parts of this notebook were taken from the [Authoring a Multibody Simulation tutorial](https://deepnote.com/workspace/Drake-0b3b2c53-a7ad-441b-80f8-bf8350752305/project/Tutorials-2b4fc509-aef2-417d-a40d-6071dfed9199/notebook/authoring_multibody_simulation-add293478aac40a984845aa3705eefdd?). Still, it is useful to take a look at the tutorial to learn more about the concepts."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A high level overview of the different formats around describing simulations in Drake:\n",
    "- URDF and SDF are XML formats that can specify the kinematic and dynamic properties of robots or objects. SDF is the more scalable format, and it is the format that is recommended. However, they should be mostly interchangeable with small syntax differences.\n",
    "- .obj on the other hand, is a file format that describes the *geometry* of an object, such as a mesh of the surface of the object.\n",
    "  - There exist other geometry formats, such as .stl, and .dae. These are not well-supported by Drake directly, but [Meshlab](https://www.meshlab.net/), an open-source software, is a handy tool to convert common formats to a `.obj` for you.\n",
    "- [Model directives](https://drake.mit.edu/doxygen_cxx/structdrake_1_1multibody_1_1parsing_1_1_model_directive.html) are a YAML specification that makes it easy to load multiple URDF and SDF objects into a simulation. They're used at the bottom of this notebook.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The most important formats for creating multibody scenarios in Drake are the [Unified Robot Description Format (URDF)](http://wiki.ros.org/urdf) and the [Simulation Description Format (SDFormat)](http://sdformat.org/).\n",
    "\n",
    "They are both XML formats to describe robots or objects for robot simulators or visualization, and are fairly similar in syntax.\n",
    "\n",
    "In a high-level sense, you express different components of your robot using `<link>` tags and connect them via `<joint>` tags. Each `<link>` has three major subtags, `<visual>`, `<collision>`, and `<inertial>`, for its visualization, planning/collision checking, and dynamics aspects. For `<visual>` and `<collision>`, you can either use primitives (box, sphere, cylinder, etc.) or meshes (.obj) to represent the underlying geometry.\n",
    "\n",
    "The supported primitive types and the supported file types for meshes are described [here](https://drake.mit.edu/doxygen_cxx/group__geometry__file__formats.html). "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Written Question\n",
    "Compare and contrast two SDF files of the same YCB sugar box: [File 1](https://github.com/RobotLocomotion/drake/blob/8ed3a0264c521ffc1ea8861f5701b81dfb696af8/manipulation/models/ycb/sdf/004_sugar_box.sdf) and [File 2](https://github.com/RussTedrake/manipulation/blob/24f122591f72cc78931ea3f461d0c34845dc9aca/manipulation/models/hydro/004_sugar_box.sdf). File 2 uses hydroelastic contact, and file 1 uses the default point contact model.\n",
    "\n",
    "[Hydroelastic contact](https://drake.mit.edu/doxygen_cxx/group__hydroelastic__user__guide.html) in Drake can be used to create compliant collision geometries. This is a great tool for improving the contact dynamics between simulated objects, compared to traditional rigid contact geometries.\n",
    "\n",
    "Answer the following questions in your written submission.\n",
    "\n",
    "**Part (a):**  How is the collision geometry (not contact models!) different in File 1 than File 2? What tags enable hydroelastic contact in File 2?\n",
    "\n",
    "**Part (b):**  In both files, we use the sugar box obj mesh as the visual geometry. For File 2, we could have used the obj mesh as the collision geometry too. In one sentence, why might we prefer to specify the collision geometry using primitives, rather than using the obj mesh? "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Adding your own models\n",
    "\n",
    "Now, you will add an object of your choosing into simulation! You have a lot of freedom here. Ideally, this object can be used in your final project. It doesn't have to be an object your robot can pick up; it can be anything you want in your simulation. \n",
    "\n",
    "- The only requirement is it cannot be directly taken from the existing models in Drake without modification.\n",
    "- Your model file can be in any of the formats supported by the [Parser](https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_parser.html), but we recommend using SDF. Your model should use hydroelastic contact.\n",
    "\n",
    "One option is to find a 3D model online, such as at [TurboSquid](https://www.turbosquid.com/3d-model/free), and convert it into a .obj file. The .obj file can then be directly turned into a SDF file by using the below terminal command, documented [here](https://drake.mit.edu/pydrake/pydrake.multibody.mesh_to_model.html). On Deepnote, you can open a terminal on the left bar and run the command after you have uploaded your .obj file. \n",
    "```sh\n",
    "python3 -m pydrake.multibody.mesh_to_model --scale=1.0 path/to/mesh.obj\n",
    "```\n",
    "\n",
    "We have a more advanced version of this tool available in `manipulation/create_sdf_from_mesh.py`, which additionally supports convex decomposition of a nonconvex mesh and mesh simplification. This isn't needed for this problem, but may be very useful for your project.\n",
    "\n",
    "Another option is to purely use SDF and geometric primitives to compose primitives (and maybe meshes) to build up your object. \n",
    "\n",
    "Here's some more examples of various SDF files to use as inspiration or a guide: \n",
    "- [Rubik's Cube](https://deepnote.com/workspace/Manipulation-ac8201a1-470a-4c77-afd0-2cc45bc229ff/project/02-Lets-get-you-a-robot-8f86172b-b597-4ceb-9bad-92d11ac7a6cc/notebook/rubiks_cube-35164353b13d4a47910ca14e588c74d6?)\n",
    "- [Drake examples](https://github.com/RobotLocomotion/drake/tree/master/examples), which includes things such as a bouncing ball. \n",
    "- If there was a [final project](https://manipulation.csail.mit.edu/misc.html#projects) that had objects you'd like to use, see if their code is open source or ask!\n",
    "\n",
    "Once you have created your model file, fill in the **absolute path** below. On Deepnote, there is an option on the left bar to copy paths into your clipboard. \n",
    "\n",
    "**Part (c):** In your written submission, briefly describe what your object is. Take a screenshot of your object on the table in simulation (the final code cell). Your screenshot should include red contact force arrows and not green contact force arrows, which indicate that you are using hydroelastic contact. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "your_model_filename = \"\"  # Write the absolute path to your file here"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "It is very helpful to visualize and check your model prior to putting it into simulation. Run the below cell to do so. \n",
    "\n",
    "Under the `Scene > drake` dropdown in MeshCat, you can enable viewing 'illustration', 'inertia', and 'proximity' to see the visual model, inertia matrix, and collision model, respectively. You can also adjust the poses of the links in the sliders. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "if your_model_filename:\n",
    "    visualizer = ModelVisualizer(meshcat=meshcat)\n",
    "    visualizer.parser().AddModels(your_model_filename)\n",
    "    visualizer.Run(loop_once=not running_as_notebook)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, we'll create the simulation with your model. The simulation is replayable in MeshCat. A YCB sugar box has been added to the simulation as a reference, but you may comment it out and remove it.\n",
    "\n",
    "You will have to replace the link specified in the model directives based on your file. You may have to adjust the starting pose of your model, physical properties of your model, and/or the size of the table (defined at the top of the file) in order to not have your object fall through the world."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def create_scene():\n",
    "    scenario_data = f\"\"\"\n",
    "directives:\n",
    "- add_model:\n",
    "    name: table_top\n",
    "    file: file://{table_top_sdf_file}\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: table_top::table_top_center\n",
    "- add_model:\n",
    "    name: sugar_box\n",
    "    file: package://manipulation/hydro/004_sugar_box.sdf\n",
    "    default_free_body_pose:\n",
    "        base_link_sugar:\n",
    "            translation: [0, 0, 0.75]\n",
    "            rotation: !Rpy {{ deg: [42, 33, 18] }}    \n",
    "\"\"\"\n",
    "    if your_model_filename:\n",
    "        scenario_data += f\"\"\"\n",
    "- add_model:\n",
    "    name: your_model\n",
    "    file: file://{your_model_filename}\n",
    "    default_free_body_pose:\n",
    "        YOUR_MODEL_LINK_NAME: # Change here!\n",
    "            translation: [0, 0, 0.5]\n",
    "            rotation: !Rpy {{ deg: [42, 33, 18] }}    \n",
    "\"\"\"\n",
    "\n",
    "    scenario = load_scenario(data=scenario_data)\n",
    "    station = MakeHardwareStation(scenario, meshcat)\n",
    "\n",
    "    simulator = Simulator(station)\n",
    "    meshcat.StartRecording()\n",
    "    simulator.AdvanceTo(2.0 if running_as_notebook else 0.1)\n",
    "    meshcat.PublishRecording()\n",
    "\n",
    "\n",
    "create_scene()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## How will this notebook be Graded?\n",
    "\n",
    "If you are enrolled in the class, this notebook will be graded using [Gradescope](www.gradescope.com). You should have gotten the enrollement code on our announcement in Piazza. \n",
    "\n",
    "This notebook only consists of written questions, so please submit all responses in your PDF submission to Gradescope. For this exercise, you should have completed the following subparts:\n",
    "- Correct answers to part (a)\n",
    "- Correct answers to part (b)\n",
    "- A screenshot and description of your added object for part (c)"
   ]
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}